/**
*@note Copyright 2012-2022 CETHIK All Rights Reserved.
*@brief 数据获取类实现文件 
*@author tuyinan
*@data 2022-04-xx
*/
#include <costmap_2d/data_collector.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2_ros/message_filter.h>

namespace costmap_2d {

DataCollector::DataCollector(std::string config) {
  ros::NodeHandle nh("~/data_collector"), g_nh;

  configNode_ = new ConfigFile(config);
  configNode_->ExtractKeys();

  global_frame_ = (*configNode_).getValueOfKeyAsString("global_frame", "/map");
  // get the parameters for the specific topic
  std::string pts_topic, odom_topic, map_topic, data_type;

  pts_topic = (*configNode_).getValueOfKeyAsString("pts_topic", "/points");
  odom_topic = (*configNode_).getValueOfKeyAsString("odom_topic", "/odom");
  map_topic = (*configNode_).getValueOfKeyAsString("map_topic", "/map");

  first_map_only_ = (*configNode_).getValueOfKey<bool>("first_map_only", false);

  // odom_sub_thread_ = new
  // boost::thread(boost::bind(&DataCollector::createOdomSubThread, this, nh,
  // odom_topic));
  odom_sub_thread_ =
      new boost::thread(boost::bind(&DataCollector::getOdomByTfThread, this));
  odom_sub_thread_->detach();
  map_sub_thread_ = new boost::thread(
      boost::bind(&DataCollector::createMapSubThread, this, nh, map_topic));
  map_sub_thread_->detach();
  pts_sub_thread_ = new boost::thread(
      boost::bind(&DataCollector::createPtsSubThread, this, nh, pts_topic));
  pts_sub_thread_->detach();
}

DataCollector::~DataCollector() {}

void DataCollector::getOdomByTfThread() {
  tf::TransformListener tf_;
  tf::StampedTransform baselink2mapTF;
  // make sure transform!!!
  while (ros::ok()) {
    odom_mutex_.lock();
    try {
      tf_.lookupTransform("/map", "/base_link", ros::Time(0), baselink2mapTF);
      odom_data_.position.x = baselink2mapTF.getOrigin().x();
      odom_data_.position.y = baselink2mapTF.getOrigin().y();
      odom_data_.position.z = baselink2mapTF.getOrigin().z();

      odom_data_.orientation.x = baselink2mapTF.getRotation().x();
      odom_data_.orientation.y = baselink2mapTF.getRotation().y();
      odom_data_.orientation.z = baselink2mapTF.getRotation().z();
      odom_data_.orientation.w = baselink2mapTF.getRotation().w();
      odom_recevied_time_ = ros::Time::now();
      odom_flag_ = true;
    } catch (std::exception& e) {
      // cout<<"Transform Error Happaned!!!"<<endl;
      int a = 1;
    }
    odom_mutex_.unlock();
  }
}
// 获取机器人位置
void DataCollector::createOdomSubThread(ros::NodeHandle& nh,
                                        std::string odom_topic) {
  sub_odom_ = nh.subscribe(odom_topic, 1, &DataCollector::odomCallback, this);
  // 使得程序持续运行
  ros::Rate r(10);
  while (nh.ok()) {
    ros::spinOnce();
    r.sleep();
  }
}

// 通过topic去得到map数据
void DataCollector::createMapSubThread(ros::NodeHandle& nh,
                                       std::string map_topic) {
  sub_map_ = nh.subscribe(map_topic, 1, &DataCollector::mapCallback, this);
  // 使得程序持续运行
  ros::Rate r(10);
  while (nh.ok()) {
    ros::spinOnce();
    r.sleep();
  }
}

// 获取点云数据
void DataCollector::createPtsSubThread(ros::NodeHandle& nh,
                                       std::string pts_topic) {
  sub_pts_ =
      nh.subscribe(pts_topic, 1, &DataCollector::pointCloud2Callback, this);
  // 使得程序持续运行
  ros::Rate r(10);
  while (nh.ok()) {
    ros::spinOnce();
    r.sleep();
  }
}

void DataCollector::mapCallback(
    const nav_msgs::OccupancyGridConstPtr& new_map) {
  map_mutex_.lock();
  map_data_.time = time(0);
  map_data_.height = new_map->info.height;
  map_data_.width = new_map->info.width;
  map_data_.resolution = new_map->info.resolution;
  if (!map_data_.data.empty()) {
    map_data_.data.clear();
  }
  map_data_.data.assign(new_map->data.begin(), new_map->data.end());

  map_data_.origin.position.x = new_map->info.origin.position.x;
  map_data_.origin.position.y = new_map->info.origin.position.y;
  map_data_.origin.orientation.w = new_map->info.origin.orientation.w;
  map_data_.origin.orientation.x = new_map->info.origin.orientation.x;
  map_data_.origin.orientation.y = new_map->info.origin.orientation.y;
  map_data_.origin.orientation.z = new_map->info.origin.orientation.z;
  // shutdown the map subscrber if firt_map_only_ flag is on
  if (first_map_only_) {
    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
    sub_map_.shutdown();
  }
  map_recevied_time_ = ros::Time::now();
  map_mutex_.unlock();
}

void DataCollector::pointCloud2Callback(
    const sensor_msgs::PointCloud2ConstPtr& message) {
  // buffer the point cloud
  // 这里转换为可用的message
  pts_mutex_.lock();
  if (pt_data_.points.size() > 0.0) {
    pt_data_.points.clear();
  }
  tf::TransformListener tf_;
  tf::StampedTransform camera2baseTF;
  // make sure transform!!!
  while (ros::ok()) {
    try {
      tf_.lookupTransform("/base_link", "xtion_rgb_optical_frame", ros::Time(0),
                          camera2baseTF);
      break;
    } catch (std::exception& e) {
      // cout<<"Transform Error Happaned!!!"<<endl;
      int a = 1;
    }
  }

  costmap_2d::Point32 pt;
  //  iter 转换！！！
  pcl::PointCloud<pcl::PointXYZI>::Ptr terrain_cloud_ =
      pcl::PointCloud<pcl::PointXYZI>::Ptr(
          new pcl::PointCloud<pcl::PointXYZI>());
  pcl::PointCloud<pcl::PointXYZI>::Ptr terrain_cloud_ds =
      pcl::PointCloud<pcl::PointXYZI>::Ptr(
          new pcl::PointCloud<pcl::PointXYZI>());

  pcl::fromROSMsg(*message, *terrain_cloud_);

  pcl::VoxelGrid<pcl::PointXYZI> point_ds;
  point_ds.setLeafSize(0.05, 0.05, 0.05);
  point_ds.setInputCloud(terrain_cloud_);
  point_ds.filter(*terrain_cloud_ds);
  pcl::PointCloud<pcl::PointXYZI>::iterator iter;

  for (iter = terrain_cloud_ds->begin(); iter < terrain_cloud_ds->end();
       iter++) {
    if (std::isnan(iter->x) || std::isnan(iter->y) || std::isnan(iter->z) ||
        std::isinf(iter->x) || std::isinf(iter->y) || std::isinf(iter->z)) {
      continue;
    }
    double px = iter->x, py = iter->y, pz = iter->z;
    pt.x = px;
    pt.y = py;
    pt.z = pz;
    pt_data_.points.push_back(pt);
  }
  do_transform_camera(pt_data_, camera2baseTF);
  pts_recevied_time_ = ros::Time::now();
  pts_mutex_.unlock();
}

void DataCollector::odomCallback(const nav_msgs::Odometry::ConstPtr& odom) {
  odom_mutex_.lock();
  odom_data_.position.x = odom->pose.pose.position.x;
  odom_data_.position.y = odom->pose.pose.position.y;
  odom_data_.position.z = odom->pose.pose.position.z;

  odom_data_.orientation.x = odom->pose.pose.orientation.x;
  odom_data_.orientation.y = odom->pose.pose.orientation.y;
  odom_data_.orientation.z = odom->pose.pose.orientation.z;
  odom_data_.orientation.w = odom->pose.pose.orientation.w;
  odom_recevied_time_ = ros::Time::now();
  odom_flag_ = true;
  odom_mutex_.unlock();
}

bool DataCollector::getPts(costmap_2d::PointCloud& pts) {
  pts_mutex_.lock();
  if (ros::Time::now() - pts_recevied_time_ < ros::Duration(0.5) &&
      !pt_data_.points.empty()) {
    pts = pt_data_;
    pts_mutex_.unlock();
    return true;
  }
  pts_mutex_.unlock();
  return false;
}

bool DataCollector::getMap(costmap_2d::OccupancyGrid& map) {
  map_mutex_.lock();
  if (!map_data_.data.empty()) {
    map = map_data_;
    map_mutex_.unlock();
    return true;
  }
  // 由于地图只发送一次所以这边这个关于时间的配置并没啥用
  // std::cout<<"map_time:"<<((ros::Time::now() - map_recevied_time_) <
  // ros::Duration(0.5)) <<std::endl; if(ros::Time::now() - map_recevied_time_ <
  // ros::Duration(0.5) && !map_data_.data.empty()){
  //     map = map_data_;
  //     map_mutex_.unlock();
  //     return true;
  // }
  map_mutex_.unlock();
  return false;
}

bool DataCollector::getOdom(costmap_2d::Pose& odom) {
  odom_mutex_.lock();
  if (ros::Time::now() - odom_recevied_time_ < ros::Duration(0.5) && odom_flag_) {
    odom = odom_data_;
    odom_mutex_.unlock();
    return true;
  }
  odom_mutex_.unlock();
  return false;
}

void DataCollector::do_transform_camera(costmap_2d::PointCloud& pts,
                                        tf::StampedTransform t_in) {
  Eigen::Transform<float, 3, Eigen::Isometry> t0 =
      Eigen::Translation3f(0, 0, 0) * Eigen::Quaternion<float>(1, 0, 0, 0);
  Eigen::Transform<float, 3, Eigen::Isometry> t =
      Eigen::Translation3f(t_in.getOrigin().x(), t_in.getOrigin().y(),
                           t_in.getOrigin().z()) *
      Eigen::Quaternion<float>(t_in.getRotation().w(), t_in.getRotation().x(),
                               t_in.getRotation().y(), t_in.getRotation().z());

  t = t0 * t;
  for (size_t i = 0; i < pts.points.size(); i++) {
    Eigen::Vector3f point =
        t * Eigen::Vector3f((pts.points)[i].x, (pts.points)[i].y,
                            (pts.points)[i].z);
    (pts.points)[i].x = point.x();
    (pts.points)[i].y = point.y();
    (pts.points)[i].z = point.z();
  }
}

}  // namespace costmap_2d